#include "attitude_visualizer.h"
#include <iostream>
#include <iomanip>
#include <cmath>
#include <sstream>

#ifdef _WIN32
#include <windows.h>
#else
#include <unistd.h>
#endif

// 构造函数
AttitudeVisualizer::AttitudeVisualizer() : isRunning_(false) {}

// 析构函数
AttitudeVisualizer::~AttitudeVisualizer() { stop(); }

// 更新姿态数据
void AttitudeVisualizer::updateAttitude(const MSP::AttitudeData &attitude) {
  std::lock_guard<std::mutex> lock(dataMutex_);
  attitude_ = attitude;
}

// 更新原始IMU数据
void AttitudeVisualizer::updateRawImu(const MSP::RawImuData &imu) {
  std::lock_guard<std::mutex> lock(dataMutex_);
  rawImu_ = imu;
}

// 启动可视化
bool AttitudeVisualizer::start() {
  if (isRunning_) {
    return false;
  }

  isRunning_ = true;
  visualizeThread_ =
      std::thread(&AttitudeVisualizer::visualizeThreadFunc, this);
  return true;
}

// 停止可视化
void AttitudeVisualizer::stop() {
  if (!isRunning_) {
    return;
  }

  isRunning_ = false;
  if (visualizeThread_.joinable()) {
    visualizeThread_.join();
  }
}

// 检查是否正在运行
bool AttitudeVisualizer::isRunning() const { return isRunning_; }

// 获取当前姿态数据
MSP::AttitudeData AttitudeVisualizer::getAttitude() const {
  std::lock_guard<std::mutex> lock(dataMutex_);
  return attitude_;
}

// 获取当前IMU原始数据
MSP::RawImuData AttitudeVisualizer::getRawImu() const {
  std::lock_guard<std::mutex> lock(dataMutex_);
  return rawImu_;
}

// 可视化线程函数
void AttitudeVisualizer::visualizeThreadFunc() {
  while (isRunning_) {
    // 获取当前姿态数据的副本
    MSP::AttitudeData attitude;
    MSP::RawImuData imu;

    {
      std::lock_guard<std::mutex> lock(dataMutex_);
      attitude = attitude_;
      imu = rawImu_;
    }

    // 在终端上显示姿态
    displayTerminalAttitude();

    // 休眠一段时间，避免频繁刷新
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
}

// 在终端中显示姿态数据
void AttitudeVisualizer::displayTerminalAttitude() {
  // 清屏
  clearScreen();

  // 获取最新的姿态数据
  MSP::AttitudeData attitude;
  MSP::RawImuData imu;

  {
    std::lock_guard<std::mutex> lock(dataMutex_);
    attitude = attitude_;
    imu = rawImu_;
  }

  // 打印标题
  std::cout << "======== Betaflight 姿态可视化 ========" << std::endl
            << std::endl;

  // 显示姿态角度
  std::cout << "姿态角 (度): " << std::endl;
  std::cout << "  Roll  (横滚): " << std::setw(8) << std::fixed
            << std::setprecision(2) << attitude.roll << std::endl;
  std::cout << "  Pitch (俯仰): " << std::setw(8) << std::fixed
            << std::setprecision(2) << attitude.pitch << std::endl;
  std::cout << "  Yaw   (偏航): " << std::setw(8) << std::fixed
            << std::setprecision(2) << attitude.yaw << std::endl;
  std::cout << std::endl;

  // 显示原始IMU数据
  std::cout << "原始IMU数据: " << std::endl;
  std::cout << formatImuData(imu) << std::endl;
  std::cout << std::endl;

  // 绘制简单的人工水平仪
  int width = 60;
  int height = 20;
  std::cout << "人工水平仪:" << std::endl;
  drawArtificialHorizon(attitude.roll, attitude.pitch, width, height);

  std::cout << std::endl;
  std::cout << "按Ctrl+C退出" << std::endl;
}

// 在终端上绘制简单的人工水平仪
void AttitudeVisualizer::drawArtificialHorizon(float roll, float pitch,
                                               int width, int height) {
  // 创建水平仪
  std::vector<std::string> horizon(height, std::string(width, ' '));

  // 水平线的位置
  int horizonLine = height / 2 - static_cast<int>(pitch / 5.0f);

  // 绘制边框
  for (int i = 0; i < width; i++) {
    horizon[0][i] = '-';
    horizon[height - 1][i] = '-';
  }

  for (int i = 0; i < height; i++) {
    horizon[i][0] = '|';
    horizon[i][width - 1] = '|';
  }

  // 计算倾斜角对应的斜率
  float rollRad = roll * M_PI / 180.0f;
  float slope = tan(rollRad);

  // 确保水平线在可视范围内
  horizonLine = std::max(1, std::min(height - 2, horizonLine));

  // 绘制水平线
  for (int x = 1; x < width - 1; x++) {
    // 相对于中心的位置
    int relX = x - width / 2;

    // 计算当前x坐标处的y偏移
    int yOffset = static_cast<int>(relX * slope);

    // 计算实际y坐标
    int y = horizonLine + yOffset;

    // 确保y在可视范围内
    y = std::max(1, std::min(height - 2, y));

    // 在水平线以上填充天空符号
    for (int i = 1; i < y; i++) {
      horizon[i][x] = ' ';
    }

    // 绘制水平线
    horizon[y][x] = '-';

    // 在水平线以下填充地面符号
    for (int i = y + 1; i < height - 1; i++) {
      horizon[i][x] = '.';
    }
  }

  // 绘制中心指示符
  int centerX = width / 2;
  int centerY = height / 2;
  horizon[centerY][centerX - 2] = '+';
  horizon[centerY][centerX + 2] = '+';
  horizon[centerY - 2][centerX] = '+';
  horizon[centerY + 2][centerX] = '+';

  // 输出整个水平仪
  for (const std::string &line : horizon) {
    std::cout << line << std::endl;
  }
}

// 将原始IMU数据转换为可读格式
std::string AttitudeVisualizer::formatImuData(
    const MSP::RawImuData &imu) const {
  std::stringstream ss;

  ss << "  加速度计 (ACC): X=" << std::setw(6) << imu.acc[0]
     << ", Y=" << std::setw(6) << imu.acc[1] << ", Z=" << std::setw(6)
     << imu.acc[2] << std::endl;

  ss << "  陀螺仪 (GYRO):  X=" << std::setw(6) << imu.gyro[0]
     << ", Y=" << std::setw(6) << imu.gyro[1] << ", Z=" << std::setw(6)
     << imu.gyro[2] << std::endl;

  ss << "  磁力计 (MAG):   X=" << std::setw(6) << imu.mag[0]
     << ", Y=" << std::setw(6) << imu.mag[1] << ", Z=" << std::setw(6)
     << imu.mag[2];

  return ss.str();
}

// 清屏函数
void AttitudeVisualizer::clearScreen() const {
#ifdef _WIN32
  // Windows平台
  HANDLE hConsole = GetStdHandle(STD_OUTPUT_HANDLE);
  COORD coordScreen = {0, 0};
  DWORD cCharsWritten;
  CONSOLE_SCREEN_BUFFER_INFO csbi;

  GetConsoleScreenBufferInfo(hConsole, &csbi);
  DWORD dwConSize = csbi.dwSize.X * csbi.dwSize.Y;

  FillConsoleOutputCharacter(hConsole, ' ', dwConSize, coordScreen,
                             &cCharsWritten);
  GetConsoleScreenBufferInfo(hConsole, &csbi);
  FillConsoleOutputAttribute(hConsole, csbi.wAttributes, dwConSize, coordScreen,
                             &cCharsWritten);
  SetConsoleCursorPosition(hConsole, coordScreen);
#else
  // Unix平台
  std::cout << "\033[2J\033[1;1H";
#endif
}